;
; Ephi - simulation of magnetic fields and particles
; Copyright (C) 2007 Indrek Mandre <indrek(at)mare.ee>
; For more information please see http://www.mare.ee/indrek/ephi/
;
; This program is free software; you can redistribute it and/or modify
; it under the terms of the GNU General Public License as published by
; the Free Software Foundation; either version 2 of the License, or
; (at your option) any later version.
;
; This program is distributed in the hope that it will be useful,
; but WITHOUT ANY WARRANTY; without even the implied warranty of
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
; GNU General Public License for more details.
;
; You should have received a copy of the GNU General Public License along
; with this program; if not, write to the Free Software Foundation, Inc.,
; 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
;

global addbfield_sse_double
global addbfield_sse_float

; extern "C" void addbfield_sse_double (double *posv, double *kmiv, double *begin, double *end, double *resbuf)
addbfield_sse_double:
    push ebp
    mov ebp, esp
    push esi

    mov esi, [ebp+16] ; take begin into esi
    mov eax, [ebp+8] ; take posv into eax
    mov ecx, [ebp+12] ; take kmiv into eax
    mov edx, [ebp+24] ; take resbuf into edx

    ; sanity check that we are not passed an empty dataset
    cmp esi, [ebp+20]
    jne .loop
    pop esi
    leave
    ret

.loop:
    ;;;;;;;;;;;;;;;;;;;;;;;; step1: load pos and middle
    ;; load x0; // x0 = p.x
    ;; load x1; // x1 = p.y
    ;; load x2; // x2 = p.z
    ;;
    ;; load x3; // x3 = m.x
    ;; load x4; // x4 = m.y
    ;; load x5; // x5 = m.z
    movapd xmm0, [eax]
    movapd xmm3, [esi]

    movapd xmm1, [eax + 16]
    movapd xmm4, [esi + 16]

    movapd xmm2, [eax + 32]
    movapd xmm5, [esi + 32]

    ;;;;;;;;;;;;;;;;;;;;;;;; step2: find vector r
    ;; x3 = x3 - x0; // x3 = m.x - p.x (r.x)
    ;; x4 = x4 - x1; // x4 = m.y - p.y (r.y)
    ;; x5 = x5 - x2; // x5 = m.z - p.z (r.z)
    subpd xmm3, xmm0
    subpd xmm4, xmm1
    subpd xmm5, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step3: find the indiviaul multiplication elements of the r vectors
    ;; x0 = x3; x0 = x0 * x0; // x0 = r.x * r.x
    ;; x1 = x4; x1 = x1 * x1; // x1 = r.y * r.y
    ;; x2 = x5; x2 = x2 * x2; // x2 = r.z * r.z
    movapd xmm0, xmm3
    mulpd xmm0, xmm0
    movapd xmm1, xmm4
    mulpd xmm1, xmm1
    movapd xmm2, xmm5
    mulpd xmm2, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step4: add up the multipliation elements to get the dot product value
    ;; x0 = x0 + x1; // x0 = r.x * r.x + r.y * r.y
    ;; x0 = x0 + x2; // x0 = r.x * r.x + r.y * r.y + r.z * r.z
    addpd xmm0, xmm1
    addpd xmm0, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step5: calculate the length of r
    ;; x1 = x0; x1 = sqrt(x1); // x1 = |r|
    sqrtpd xmm1, xmm0

    ;;;;;;;;;;;;;;;;;;;;;;;; step6: get |r|^3
    ;; x0 = x0 * x1; // x0 = |r|^3
    mulpd xmm0, xmm1

    ;;;;;;;;;;;;;;;;;;;;;;;; step7: load KmI
    ;; load x6; // x6 = (KmI, KmI)
    movapd xmm6, [ecx]

    ;;;;;;;;;;;;;;;;;;;;;;;; step8: divide KmI with the |r|^3 to find the scalar
    ;; x6 = x6 / x0; // x6 = mod
    divpd xmm6, xmm0

    ;;;;;;;;;;;;;;;;;;;;;;;; step9: mul modulator with the r vector
    ;; x3 = x3 * x6
    ;; x4 = x4 * x6
    ;; x5 = x5 * x6
    mulpd xmm3, xmm6
    mulpd xmm4, xmm6
    mulpd xmm5, xmm6

    ;;;;;;;;;;;;;;;;;;;;;;;; step 10: load slope
    ;; load x0; // x0 = s.x
    ;; load x1; // x1 = s.y
    ;; load x2; // x2 = s.z
    movapd xmm0, [esi + 48]
    movapd xmm1, [esi + 64]
    movapd xmm2, [esi + 80]

    ;;;;;;;;;;;;;;;;;;;;;;;; step 11: the cross product
    ;; x6 = x1 * x5; // x6 = s.y * r.z
    ;; x7 = x2 * x4; // x7 = s.z * r.y
    ;; x7 = x7 - x6; // x7 = s.z * r.y - s.y * r.z = R.x
    ;;
    ;; x2 = x2 * x3; // x2 = s.z * r.x
    ;; x5 = x5 * x0; // x5 = s.x * r.z
    ;; x5 = x5 - x2; // x5 = s.x * r.z - s.z * r.x = R.y
    ;;
    ;; x0 = x0 * x4; // x0 = s.x * r.y
    ;; x1 = x1 * x3; // x1 = s.y * r.x
    ;; x1 = x1 - x0; // x1 = s.y * r.x - s.x * r.y = R.z
    movapd xmm6, xmm1
    mulpd xmm6, xmm5
    movapd xmm7, xmm2
    mulpd xmm7, xmm4
    subpd xmm7, xmm6

    mulpd xmm2, xmm3
    mulpd xmm5, xmm0
    subpd xmm5, xmm2

    mulpd xmm0, xmm4
    mulpd xmm1, xmm3
    subpd xmm1, xmm0

    ;; result pairs are now in (xmm7, xmm5, xmm1)
    movapd xmm2, [edx]
    movapd xmm3, [edx + 16]
    movapd xmm4, [edx + 32]

    addpd xmm2, xmm7
    addpd xmm3, xmm5
    addpd xmm4, xmm1

    movapd [edx], xmm2
    movapd [edx + 16], xmm3
    movapd [edx + 32], xmm4

    ; moving to the next iteration
    add esi, 96
    cmp esi, [ebp+20]
    jne .loop

    pop esi
    leave
    ret

; extern "C" void addbfield_sse_float (double *posv, double *kmiv, double *begin, double *end, double *resbuf)
addbfield_sse_float:
    push ebp
    mov ebp, esp
    push esi

    mov esi, [ebp+16] ; take begin into esi
    mov eax, [ebp+8] ; take posv into eax
    mov ecx, [ebp+12] ; take kmiv into eax
    mov edx, [ebp+24] ; take resbuf into edx

    ; sanity check that we are not passed an empty dataset
    cmp esi, [ebp+20]
    jne .loop
    pop esi
    leave
    ret

.loop:
    ;;;;;;;;;;;;;;;;;;;;;;;; step1: load pos and middle
    ;; load x0; // x0 = p.x
    ;; load x1; // x1 = p.y
    ;; load x2; // x2 = p.z
    ;;
    ;; load x3; // x3 = m.x
    ;; load x4; // x4 = m.y
    ;; load x5; // x5 = m.z
    movaps xmm0, [eax]
    movaps xmm3, [esi]

    movaps xmm1, [eax + 16]
    movaps xmm4, [esi + 16]

    movaps xmm2, [eax + 32]
    movaps xmm5, [esi + 32]

    ;;;;;;;;;;;;;;;;;;;;;;;; step2: find vector r
    ;; x3 = x3 - x0; // x3 = m.x - p.x (r.x)
    ;; x4 = x4 - x1; // x4 = m.y - p.y (r.y)
    ;; x5 = x5 - x2; // x5 = m.z - p.z (r.z)
    subps xmm3, xmm0
    subps xmm4, xmm1
    subps xmm5, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step3: find the indiviaul multiplication elements of the r vectors
    ;; x0 = x3; x0 = x0 * x0; // x0 = r.x * r.x
    ;; x1 = x4; x1 = x1 * x1; // x1 = r.y * r.y
    ;; x2 = x5; x2 = x2 * x2; // x2 = r.z * r.z
    movaps xmm0, xmm3
    mulps xmm0, xmm0
    movaps xmm1, xmm4
    mulps xmm1, xmm1
    movaps xmm2, xmm5
    mulps xmm2, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step4: add up the multipliation elements to get the dot product value
    ;; x0 = x0 + x1; // x0 = r.x * r.x + r.y * r.y
    ;; x0 = x0 + x2; // x0 = r.x * r.x + r.y * r.y + r.z * r.z
    addps xmm0, xmm1
    addps xmm0, xmm2

    ;;;;;;;;;;;;;;;;;;;;;;;; step5: calculate the length of r
    ;; x1 = x0; x1 = sqrt(x1); // x1 = |r|
    sqrtps xmm1, xmm0

    ;;;;;;;;;;;;;;;;;;;;;;;; step6: get |r|^3
    ;; x0 = x0 * x1; // x0 = |r|^3
    mulps xmm0, xmm1

    ;;;;;;;;;;;;;;;;;;;;;;;; step7: load KmI
    ;; load x6; // x6 = (KmI, KmI)
    movaps xmm6, [ecx]

    ;;;;;;;;;;;;;;;;;;;;;;;; step8: divide KmI with the |r|^3 to find the scalar
    ;; x6 = x6 / x0; // x6 = mod
    divps xmm6, xmm0

    ;;;;;;;;;;;;;;;;;;;;;;;; step9: mul modulator with the r vector
    ;; x3 = x3 * x6
    ;; x4 = x4 * x6
    ;; x5 = x5 * x6
    mulps xmm3, xmm6
    mulps xmm4, xmm6
    mulps xmm5, xmm6

    ;;;;;;;;;;;;;;;;;;;;;;;; step 10: load slope
    ;; load x0; // x0 = s.x
    ;; load x1; // x1 = s.y
    ;; load x2; // x2 = s.z
    movaps xmm0, [esi + 48]
    movaps xmm1, [esi + 64]
    movaps xmm2, [esi + 80]

    ;;;;;;;;;;;;;;;;;;;;;;;; step 11: the cross product
    ;; x6 = x1 * x5; // x6 = s.y * r.z
    ;; x7 = x2 * x4; // x7 = s.z * r.y
    ;; x7 = x7 - x6; // x7 = s.z * r.y - s.y * r.z = R.x
    ;;
    ;; x2 = x2 * x3; // x2 = s.z * r.x
    ;; x5 = x5 * x0; // x5 = s.x * r.z
    ;; x5 = x5 - x2; // x5 = s.x * r.z - s.z * r.x = R.y
    ;;
    ;; x0 = x0 * x4; // x0 = s.x * r.y
    ;; x1 = x1 * x3; // x1 = s.y * r.x
    ;; x1 = x1 - x0; // x1 = s.y * r.x - s.x * r.y = R.z
    movaps xmm6, xmm1
    mulps xmm6, xmm5
    movaps xmm7, xmm2
    mulps xmm7, xmm4
    subps xmm7, xmm6

    mulps xmm2, xmm3
    mulps xmm5, xmm0
    subps xmm5, xmm2

    mulps xmm0, xmm4
    mulps xmm1, xmm3
    subps xmm1, xmm0

    ;; result pairs are now in (xmm7, xmm5, xmm1)
    movaps xmm2, [edx]
    movaps xmm3, [edx + 16]
    movaps xmm4, [edx + 32]

    addps xmm2, xmm7
    addps xmm3, xmm5
    addps xmm4, xmm1

    movaps [edx], xmm2
    movaps [edx + 16], xmm3
    movaps [edx + 32], xmm4

    ; moving to the next iteration
    add esi, 96
    cmp esi, [ebp+20]
    jne .loop

    pop esi
    leave
    ret

